#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import time
import numpy as np
from datetime import datetime
from prometheus_msgs.msg import DroneState,PositionReference,ControlCommand
from mavros_msgs.srv import CommandBool, SetMode
from std_msgs.msg import Bool
from prometheus_msgs.msg import MultiArucoInfo
from prometheus_msgs.msg import ArucoInfo
import sys, select, termios, tty
import local_pyhop as ph

#工具函数
############################################################

#pythond的标准键盘获取函数
settings = termios.tcgetattr(sys.stdin)
def getKey():
	tty.setraw(sys.stdin.fileno())
	rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
	if rlist:
		key = sys.stdin.read(1)
	else:
		key = ''
	termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
	return key


#信息获取函数
def get_infor():
	global task,area,target
	while 1:
		print("Please input 0 to confirm!")
		while 1:
			task = getKey()
			if task == '0':
				print("Execute the scout_area_task")
				task = 'scout_area_task'
				break
		x_min = int(input('Please input object region ：\nx_min = '))
		x_max = int(input('x_max = '))
		y_min = int(input('y_min = '))
		y_max = int(input('y_max = '))
		area = [[x_min, x_max], [y_min, y_max]]
		target = int(input('Please input the target number ： '))
		X = abs(area[0][1] - area[0][0])
		Y = abs(area[1][1] - area[1][0])
		if X*Y > 0 and X*Y <= 100:
			break
		else:
			print('Input aera too large！Please input again!')
	tasks = [(task,area,target)]
	return tasks

#状态获取函数
def get_state(state,i):
	state.pos = _DroneState.position
	state.mode = _DroneState.mode
	state.pos_land = [5, 0, 1]
	state.target_num = np.full(8,None)
	state.target = multi_aruco_info
	if i == 0:
		return state
	else:
		state.target_num[i-1] = multi_aruco_info.num_arucos
		return state


#无人机订阅和发布的消息
############################################################

#订阅解锁与模式切换服务
arming_client = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
set_mode_client = rospy.ServiceProxy('/mavros/set_mode', SetMode)

#订阅话题
mav_pos_ = np.zeros(3)
def state_cb(msg):
	global _DroneState,mav_pos_
	_DroneState = msg
	mav_pos_ = np.array([msg.position[0], msg.position[1], msg.position[2]])
_DroneState = rospy.Subscriber("/prometheus/drone_state", DroneState, state_cb)


multi_aruco_pos_enu = np.zeros(3)
multi_aruco_info = MultiArucoInfo()
def multi_aruco_cb(msg):
	global multi_aruco_info
	multi_aruco_info = msg
multi_aruco_sub = rospy.Subscriber("/prometheus/object_detection/multi_aruco_det", MultiArucoInfo, multi_aruco_cb)



aruco_pos_enu = np.zeros(3)
aruco_info = ArucoInfo()
def aruco_cb(msg):
	global aruco_info
	aruco_info = msg
	aruco_pos_enu[0] = mav_pos_[0] - aruco_info.position[1]
	aruco_pos_enu[1] = mav_pos_[1] - aruco_info.position[0]
	#相机安装在无人机下方10cm处，需减去该偏差
	aruco_pos_enu[2] = mav_pos_[2] - aruco_info.position[2] - 0.1

aruco_sub =  rospy.Subscriber("/prometheus/object_detection/aruco_det", ArucoInfo, aruco_cb)


#声明发布的话题
move_pub = rospy.Publisher("/prometheus/control_command", ControlCommand, queue_size=10)
switch_pub = rospy.Publisher("/prometheus/object_detection/aruco_navigation_switch", Bool, queue_size=1)


#初始化参数
Command_Now = ControlCommand()
Command_Now.Mode = 0
Command_Now.Command_ID = 0
Command_Now.source = "Point"
Command_Now.Reference_State.Move_mode = 0
Command_Now.Reference_State.Move_frame = 0
Command_Now.Reference_State.position_ref[0] = 0
Command_Now.Reference_State.position_ref[1] = 0
Command_Now.Reference_State.position_ref[2] = 0
Command_Now.Reference_State.velocity_ref[0] = 0
Command_Now.Reference_State.velocity_ref[1] = 0
Command_Now.Reference_State.velocity_ref[2] = 0
Command_Now.Reference_State.acceleration_ref[0] = 0
Command_Now.Reference_State.acceleration_ref[1] = 0
Command_Now.Reference_State.acceleration_ref[2] = 0
Command_Now.Reference_State.yaw_ref = 0
sametimes = 0
v = 0.5
z = 1



#定义无人机的动作
############################################################

#无人机解锁、起飞原子任务
def arm_takeoff(rate):
	while not rospy.is_shutdown():
		arming_client(True)
		print("Vehicle arming succeed!")
		time.sleep(2)
		set_mode_client(custom_mode='OFFBOARD')
		print("Vehicle offboard succeed!")
		time.sleep(2)
		Command_Now.Mode = ControlCommand.Takeoff
		Command_Now.Command_ID = Command_Now.Command_ID + 1
		move_pub.publish(Command_Now)
		print("Vehicle takeoff succeed!")
		time.sleep(6)
		break

#无人机机动原子任务
def move(rate,pos_move):
	global sametimes
	Command_Now.Command_ID = Command_Now.Command_ID + 1
	print("Vehicle is moving!")
	while not rospy.is_shutdown():
		Command_Now.Mode = 4
		Command_Now.Reference_State.Move_mode = 0
		Command_Now.Reference_State.Move_frame = 0
		Command_Now.Reference_State.position_ref[0] = pos_move[0]
		Command_Now.Reference_State.position_ref[1] = pos_move[1]
		Command_Now.Reference_State.position_ref[2] = pos_move[2]
		Command_Now.Reference_State.yaw_ref = 0
		move_pub.publish(Command_Now)
		rate.sleep()
		if abs(_DroneState.position[0] - pos_move[0]) < 0.15 and \
				abs(_DroneState.position[1] - pos_move[1]) < 0.15:
			if sametimes > 50:
				sametimes = 0
				break
			else:
				sametimes = sametimes + 1
		else:
			sametimes = 0

#无人机降落、上锁原子任务
def land_disarm(rate):
	Command_Now.Command_ID = Command_Now.Command_ID + 1
	while not rospy.is_shutdown():
		Command_Now.Mode = ControlCommand.Land
		move_pub.publish(Command_Now)
		switch_pub.publish(False)
		print("Vehicle land and disarm succeed!")
		rate.sleep()
		break


#无人机高空扫描原子任务
def scan_high(rate,target):
	switch_pub.publish(True)
	Command_Now.Command_ID = Command_Now.Command_ID + 1
	while not rospy.is_shutdown():
		Command_Now.Mode = ControlCommand.Hold
		move_pub.publish(Command_Now)
		print("Vehicle is scanning!")
		time.sleep(2)
		if multi_aruco_info.num_arucos == 0:
			print('If Vehicle found potential targets?\nFalse')
		else:
			print('If Vehicle found potential targets?\nTure!\nFind {} potential target(s)!'
				  .format(multi_aruco_info.num_arucos))
		switch_pub.publish(False)
		time.sleep(3)
		break



#无人机扫描原子任务
def scan(rate,target):
	switch_pub.publish(True)
	Command_Now.Command_ID = Command_Now.Command_ID + 1
	while not rospy.is_shutdown():
		Command_Now.Mode = ControlCommand.Hold
		move_pub.publish(Command_Now)
		print("Vehicle is detecting!")
		time.sleep(2)
		print('Is Vehicle found target?\n{}'.format(aruco_info.detected))
		if aruco_info.detected == True:
			print('The target found is:{}\nTarget postion:[{},{},{}]'
				  .format(target,round(aruco_pos_enu[0],2),round(aruco_pos_enu[1],2),round(aruco_pos_enu[2],2)))
		switch_pub.publish(False)
		time.sleep(2)
		break
